#include "CFilter.h"

CFilter::CFilter()
{
	this->N = 6;
	this->threshold_X = 70;
	this->threshold_Y = 500;
	this->threshold_Z = 300;
	this->temp_data_state = false;
	this->target_out = true;
	this->target_lose = true;
	this->count_frame_out = 0;
	this->count_frame_lose = 0;
	dataBuff.clear();
};

void CFilter::setFilter(int Num, int threshold)
{
	this->N = Num;
	this->threshold_X = threshold;
	this->threshold_Y = threshold;
	this->threshold_Z = threshold;


	this->target_out = true;
	this->target_lose = true;
	this->temp_data_state = false;
	this->count_frame_out = 0;
	this->count_frame_lose = 0;
	dataBuff.clear();
};

void CFilter::buffClear()
{
	this->temp_data_state = false;
	this->target_out = true;
	this->target_lose = true;
	this->count_frame_out = 0;
	this->count_frame_lose = 0;
	dataBuff.clear();
};

CFilter::~CFilter(){

};

void CFilter::filter(DataStruct &ture_data, DataStruct &send_data, short &last_x)
{
	if (ture_data.Flag != 0) //有识别到目标
	{
		count_frame_lose = 0;
		if (temp_data_state == false) //flase代表当前存储区没有数据,第一帧数据存入后返回
		{
			temp_data_state = true; //true代表存储区已经有数据
			target_out = false;		//false代表目标没有离开感兴趣区间
			target_lose = false;	//false代表目标没有丢失
			dataBuff.push_back(ture_data);
			temp_data = ture_data;
			send_data = ture_data;
			last_x = send_data.X; //没有发现目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
			return;
		}
		else //数据缓冲区帧数大于等于2
		{
			int buffSize = dataBuff.size(); //将缓冲区限制在六帧
			if (buffSize == 6)
			{									  //删除第一个元素
				dataBuff.erase(dataBuff.begin()); //存储帧数到达7帧时，扔掉第一帧
				buffSize--;
			}

			if (abs(ture_data.X - temp_data.X) > this->threshold_X ||
				abs(ture_data.Y - temp_data.Y) > this->threshold_Y ||
				abs(ture_data.Z - temp_data.Z) > this->threshold_Z) //如果当前帧与前一帧中心点位置发生突变，扔掉
			{
				cout << "                        cusp happen";
				if (target_lose == true) //在六帧内丢失了目标，重新找到后的目标又超出了感兴趣区域
				{
					target_lose = false;
					count_frame_out = count_frame_lose;
					count_frame_lose = 0;
					count_frame_out++;
				}
				else //六帧内没有丢目标，但目标超出了感兴趣区域
				{
					count_frame_out++;
				}

				if (count_frame_out > N) //连续五帧检测到的目标检测到的目标都在感兴趣区间内，开始重新寻找目标
				{
					buffClear();
					send_data = ture_data;
					last_x = send_data.X; //没有发现目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
					return;
				}

				if (target_out == false) //目标首次超出感兴趣区间
				{
					target_out = true;
					send_data = temp_data;
					last_x = send_data.X; //没有发现目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
					return;
				}
				else //返回上一次的有效值
				{
					send_data = temp_data;
					last_x = send_data.X; //没有发现目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
					return;
				}
			}
			else
			{
				dataBuff.push_back(ture_data);				   //在感兴趣区间获取到目标
				if (target_out == true || target_lose == true) //六帧以内重新找回目标,temp_data保持上一次的有效值
				{
					target_out = false;
					target_lose = false;
					count_frame_out = 0;
					count_frame_lose = 0;
					send_data = ture_data;
					temp_data = ture_data;
					last_x = send_data.X; //重新找回目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
					return;
				}
				else //六帧内未丢失过目标
				{
					last_x = temp_data.X; //未丢失目标，计算速度
					temp_data = ture_data;
					send_data = ture_data;
					return;
				}
			}
		}
	}
	else //没有识别到装甲片
	{
		if (temp_data_state == false) //缓冲区没有数据，返回输入值
		{
			send_data = ture_data;
			last_x = send_data.X; //没有发现目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
			return;
		}

		count_frame_lose++;

		if (count_frame_lose < N) //丢失目标的帧数小于N
		{
			target_lose = true;
			if (target_out == true) //如果之前目标是target_out，将target_out计数次数加到target_lose上
			{
				count_frame_lose = count_frame_out;
				count_frame_out = 0;
				target_out = false;
			}
			send_data = temp_data; //目标完全丢失，六帧内发送上一次的有效数据
			last_x = send_data.X;  //没有发现目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
			return;
		}
		else //彻底丢失目标，开始重新寻找
		{
			buffClear();
			send_data = ture_data;
			last_x = send_data.X; //没有发现目标，上一帧的x值用当前帧x表示，即不进行前馈补偿
			return;
		}
	}
};
